// -*- C++ -*-
/*!
 * @file  SimRangeUrg.h
 * @brief DataConversionRTC
 * @date  $Date$
 *
 * Copyright (c) 2008, National Institute of Advanced Industrial Science and Tec                       
 * hnology (AIST). All rights reserved. This program is made available under the te                    
 * rms of the Eclipse Public License v1.0 which accompanies this distribution, and                     
 * is available at http://www.eclipse.org/legal/epl-v10.html  
 *
 * $Id$
 */

#ifndef SimRangeUrg_H
#define SimRangeUrg_H

#include <rtm/Manager.h>
#include <rtm/DataFlowComponentBase.h>
#include <rtm/CorbaPort.h>
#include <rtm/DataInPort.h>
#include <rtm/DataOutPort.h>
#include <rtm/idl/BasicDataTypeSkel.h>
/*--------------------Addition---------------------*/
#include "LaserRangeSensorDataStub.h"
#include "LaserRangeSensorDataSkel.h"
#include <iostream>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include <fstream>
#include <iomanip>

#ifdef WIN32
#include <sys/types.h>
#include <sys/timeb.h>
#include <windows.h>
#endif


using namespace std;

#define DEBUG 0
#define DEBUG_TIME 0
#define DEBUG_FILE 0
#define DEBUG_CALLBACK 0

#ifndef M_PI
#define	M_PI	3.14159265358979
#endif



#if DEBUG_CALLBACK
#include <rtm/RingBuffer.h>
class Range
	: public RTC::OnWriteConvert<RTC::TimedDoubleSeq>
{
	//ofstream ofsCallBack;
	public:
	Range(void) {};
	RTC::TimedDoubleSeq operator()(const RTC::TimedDoubleSeq& value){
		RTC::TimedDoubleSeq ret(value);
		double Time= double(value.tm.sec + 1.0e-9 * value.tm.nsec );
		struct timeval GetTime;
		if( gettimeofday(&GetTime, NULL) ) std::cout << "[ERROR] failed to gettimeofday" << std::endl;	
		double TimeNow= double(GetTime.tv_sec + 1.0e-9 * GetTime.tv_usec*1000 );
		printf("[Callback](Range) TimeNow(%f) | Time:%f  sec[%ld] nsec[%ld]\n", TimeNow, Time, value.tm.sec, value.tm.nsec);
		return ret;
	};
};
#endif
/*--------------------Addition---------------------*/
// Service implementation headers
// <rtc-template block="service_impl_h">

// </rtc-template>

// Service Consumer stub headers
// <rtc-template block="consumer_stub_h">
#include "intellirobotStub.h"

// </rtc-template>

using namespace RTC;
using namespace SensorRTC;


/*!
 * @class SimRangeUrg
 * @brief DataConversionRTC
 *
 */
class SimRangeUrg
  : public RTC::DataFlowComponentBase
{
 public:
  /*!
   * @brief constructor
   * @param manager Maneger Object
   */
  SimRangeUrg(RTC::Manager* manager);

  /*!
   * @brief destructor
   */
  ~SimRangeUrg();

  // <rtc-template block="public_attribute">
  
  // </rtc-template>

  // <rtc-template block="public_operation">
  
  // </rtc-template>

  /***
   *
   * The initialize action (on CREATED->ALIVE transition)
   * formaer rtc_init_entry() 
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
   virtual RTC::ReturnCode_t onInitialize();

  /***
   *
   * The finalize action (on ALIVE->END transition)
   * formaer rtc_exiting_entry()
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
   virtual RTC::ReturnCode_t onFinalize();

  /***
   *
   * The startup action when ExecutionContext startup
   * former rtc_starting_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);

  /***
   *
   * The shutdown action when ExecutionContext stop
   * former rtc_stopping_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);

  /***
   *
   * The activated action (Active state entry action)
   * former rtc_active_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
   virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);

  /***
   *
   * The deactivated action (Active state exit action)
   * former rtc_active_exit()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
   virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);

  /***
   *
   * The execution action that is invoked periodically
   * former rtc_active_do()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
   virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);

  /***
   *
   * The aborting action when main logic error occurred.
   * former rtc_aborting_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);

  /***
   *
   * The error action in ERROR state
   * former rtc_error_do()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);

  /***
   *
   * The reset action that is invoked resetting
   * This is same but different the former rtc_init_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
  
  /***
   *
   * The state update action that is invoked after onExecute() action
   * no corresponding operation exists in OpenRTm-aist-0.2.0
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);

  /***
   *
   * The action that is invoked when execution context's rate is changed
   * no corresponding operation exists in OpenRTm-aist-0.2.0
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);


 protected:
  // <rtc-template block="protected_attribute">
  
  // </rtc-template>

  // <rtc-template block="protected_operation">
  
  // </rtc-template>
  // Configuration variable declaration
  // <rtc-template block="config_declare">
  /*!
   * start angle of output data
   * - Name: Measurement start angle startAngle
   * - DefaultValue: -45.0
   * - Unit: [deg]
   * - Range: -45 to 270
   * - Constraint: Reflected timing ( Always )
   */
  double m_startAngle;
  /*!
   * end angle of output data
   * - Name: Measurement end angle endAngle
   * - DefaultValue: 225.0
   * - Unit: [deg]
   * - Range: -45 to 270
   * - Constraint: Reflected timing ( Always )
   */
  double m_endAngle;
  /*!
   * Number in which output data is brought together
   * - Name: groupingNumber groupingNumber
   * - DefaultValue: 1
   * - Unit: [number]
   * - Range: 1-99
   * - Constraint: Reflected timing ( Always )
   */
  int m_groupingNumber;
  /*!
   * scan angle (Model information)
   * - Name: scanAngle scanAngle
   * - DefaultValue: 270.0
   * - Unit: [deg]
   * - Range: Refer to the value among models used
   * - Constraint: Reflected timing ( inactive )
   */
  double m_scanAngle;
  /*!
   * scan angle (Model information)
   * - Name: scanStep scanStep
   * - DefaultValue: 0.25
   * - Unit: [deg]
   * - Range: Refer to the value among models used
   * - Constraint: Reflected timing ( inactive )
   */
  double m_scanStep;
  /*!
   * scan max distance (Model information)
   * - Name: scanMaxDistance scanMaxDistance
   * - DefaultValue: 10.0
   * - Unit: [m]
   * - Range: Refer to the value among models used
   * - Constraint: Reflected timing ( inactive )
   */
  double m_scanMaxDistance;
  // </rtc-template>
  // DataInPort declaration
  // <rtc-template block="inport_declare">
  TimedDoubleSeq m_InRange;
  /*!
   * Input of the distance data
   * - Type: TimedDoubleSeq
   * - Number: -
   * - Semantics: range data of sensor simulation
   * - Unit: [mm]
   */
  InPort<TimedDoubleSeq> m_InRangeIn; 
  // </rtc-template>
  //  SensorRTC::LaserRangeSensor::idl::TimedMeasuredData m_OutSensorData;
  //  OutPort<SensorRTC::LaserRangeSensor::idl::TimedMeasuredData> m_OutSensorDataOut;

  // DataOutPort declaration
  // <rtc-template block="outport_declare">
  TimedLongSeq m_out;
  /*!
   */
  OutPort<TimedLongSeq> m_outOut;
  
  // </rtc-template>

  // CORBA Port declaration
  // <rtc-template block="corbaport_declare">
  
  // </rtc-template>

  // Service declaration
  // <rtc-template block="service_declare">
  
  // </rtc-template>

  // Consumer declaration
  // <rtc-template block="consumer_declare">
  
  // </rtc-template>

 private:
  // <rtc-template block="private_attribute">
  
  // </rtc-template>

  // <rtc-template block="private_operation">
  
  // </rtc-template>
/*--------------------Addition---------------------*/

	// ========== configuration data ========== //
  double m_scanAngle_;				//!< Maintenance of configuration parameter [scanAngle]
  double m_scanStep_;					//!< Maintenance of configuration parameter [scanStep]
  double m_scanMaxDistance_;	//!< Maintenance of configuration parameter [scanMaxDistance]
  
	// ========== Flags ========== //
	// NA
	
	// ========== Constant data ========== //
	// NA
	
	// ========== Structure data ========== //
	// NA
	
	// ========== Input data ========== //
	vector <double> m_CheckDistance;	//!< Update check
	
	// ========== Output data ========== //
	// NA
	
	// ========== Other data ========== //
	// NA
	
	// ========== Time data ========== //
	#if DEBUG_TIME
	double Time_Execute;				//!< onExecute time
	double Time_Execute_old;		//!< onExecute time [old]
	#endif
	
	// ========== Debugging file ========== //
	#if DEBUG_FILE
	ofstream RangeLog;	//!< for Log file. Sensor Distance data
	#endif

	// ========== Callback ========== //
	#if DEBUG_CALLBACK
	Range m_CallbackRange;	//!< CallBack
	#endif
	
	// ========== Unit conversion ========== //
	
	double DEG2RAD( double degree ){ return degree*M_PI/180.0; };	//!< conversion function from degree to radian
	double RAD2DEG( double radian ){ return radian*180.0/M_PI; };	//!< conversion function from radian ti degree
	double TM2TIME( RTC::Time tm ){ return double(tm.sec + 1.0e-9*tm.nsec);};		//!< conversion function from tm(structure) to time
		
	// ========== Function ========== //
	
	/*!
		@brief  Get Time
		@param void
		@return RTC::Time				
	*/
	virtual RTC::Time GetTime( void ){
		RTC::Time ingTime;
  	struct timeval GetTime;  		//!< structure of time [Present]

#ifndef WIN32
		if( gettimeofday(&GetTime, NULL) )
			std::cout << "[ERROR] failed to gettimeofday" << std::endl;	
		ingTime.sec= GetTime.tv_sec;
		ingTime.nsec= GetTime.tv_usec*1000;
#else
    _timeb tv;
    _ftime_s(&tv);

     ingTime.sec = tv.time;
     ingTime.nsec = tv.millitm * 1000.0;

#endif

		return ingTime;
	};

/*--------------------Addition---------------------*/
};


extern "C"
{
  DLL_EXPORT void SimRangeUrgInit(RTC::Manager* manager);
};

#endif // SimRangeUrg_H
